#include "grasp_item.h"

GraspItem::GraspItem(const std::string& name, const BT::NodeConfig& config, rclcpp::Node* node)
    : BT::StatefulActionNode(name, config),
      node_(node),
      grasp_complete_(false),
      grasp_success_(false)
{
}

BT::PortsList GraspItem::providedPorts() { return {BT::InputPort<int>("item_id", "物品ID")}; }

BT::NodeStatus GraspItem::onStart()
{
    auto itme_id_res = getInput<int>("item_id");
    if (!itme_id_res)
    {
        // 错误信息会明确提示映射问题，方便定位
        RCLCPP_ERROR(node_->get_logger(), "获取货架ID失败：%s", itme_id_res.error().c_str());
        return BT::NodeStatus::FAILURE;
    }
    int item_id = itme_id_res.value();

    RCLCPP_INFO(node_->get_logger(), "开始导航到货架, 物品 %d", item_id);

    grasp_complete_ = false;

    // 模拟抓取过程（随机成功/失败）
    timer_ = node_->create_wall_timer(std::chrono::seconds(1),
                                     [this]()
                                     {
                                         grasp_success_ = (rand() % 100) > 40;  // 60%概率成功
                                         RCLCPP_INFO(node_->get_logger(), "抓取%s",
                                                     grasp_success_ ? "成功" : "失败");
                                         grasp_complete_ = true;
                                         timer_->cancel();
                                     });

    return BT::NodeStatus::RUNNING;
}

BT::NodeStatus GraspItem::onRunning()
{
    if (!grasp_complete_)
    {
        return BT::NodeStatus::RUNNING;
    }

    return grasp_success_ ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
}

void GraspItem::onHalted() { RCLCPP_WARN(node_->get_logger(), "抓取任务被中断"); }